#include "ti_msp_dl_config.h"
#include "Variables.h"
#include "math.h"
//用于初始化PID
void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
{
    pid->kp=p;
    pid->ki=i;
    pid->kd=d;
    pid->maxIntegral=maxI;
    pid->maxOutput=maxOut;
}

//用于进行一次PID计算  结构体，目标值，反馈值
void PID_Calc(PID *pid,float reference,float feedback)
{
    pid->lastError=pid->error;
    pid->error=reference-feedback;
    float dout=(pid->error-pid->lastError)*pid->kd;	
    float pout=pid->error*pid->kp;
    pid->integral+=pid->error*pid->ki;
    if(pid->integral > pid->maxIntegral) pid->integral=pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) pid->integral=-pid->maxIntegral;
    pid->output=pout+dout+pid->integral;
    if(pid->output > pid->maxOutput) pid->output=pid->maxOutput;
    else if(pid->output < -pid->maxOutput) pid->output=-pid->maxOutput;
}

//------------------------速度环计算  BEGIN-----------------------------//
PID Speed_Left;
PID Speed_Right;
//速度环计算
float Speed_Left_control(PID *Speed_Left, float now_Speed, float goal_Speed)
{
    PID_Calc(Speed_Left, goal_Speed, now_Speed);
    return Speed_Left->output;
}
float Speed_Right_control(PID *Speed_Right, float now_Speed, float goal_Speed)
{
    PID_Calc(Speed_Right, goal_Speed, now_Speed);
    return Speed_Right->output;
}
//------------------------角度环计算  BEGIN-----------------------------//
PID Angle;
//角度环单独写一个处理函数，因为跨+-180度
void PID_Calc_Angle(PID *pid, float reference, float feedback)
{
    pid->lastError = pid->error;
    // 角度误差计算，确保在 [-180, 180] 范围内
    float error = reference - feedback;
    if (error > 180.0f)
        error -= 360.0f;
    else if (error < -180.0f)
        error += 360.0f;
    pid->error = error;
    // 比例项
    float pout = pid->kp * pid->error;
    // 微分项
    float dout = pid->kd * (pid->error - pid->lastError);
    // 积分项，限制积分防止饱和
    pid->integral += pid->ki * pid->error;
    if (pid->integral > pid->maxIntegral)
        pid->integral = pid->maxIntegral;
    else if (pid->integral < -pid->maxIntegral)
        pid->integral = -pid->maxIntegral;
    // 输出 = 比例 + 微分 + 积分
    pid->output = pout + dout + pid->integral;
    // 输出限幅
    if (pid->output > pid->maxOutput)
        pid->output = pid->maxOutput;
    else if (pid->output < -pid->maxOutput)
        pid->output = -pid->maxOutput;
}

//速度环计算
float Angle_control(PID *Angle, float now_Angle, float goal_Angle)
{
    PID_Calc_Angle(Angle, goal_Angle, now_Angle);
    return Angle->output;
}

//------------------------位移环计算  BEGIN-----------------------------//
PID Distance_Left;
PID Distance_Right;

float Distance_Left_control(PID *Distance_Left, float now_Angle, float goal_Angle)
{
    PID_Calc(Distance_Left, goal_Angle, now_Angle);
    return Distance_Left->output;
}
float Distance_Right_control(PID *Distance_Right, float now_Angle, float goal_Angle)
{
    PID_Calc(Distance_Right, goal_Angle, now_Angle);
    return Distance_Right->output;
}


//----------------------灰度环计算  BEGIN-------------------------------//
PID Gray;

float Gray_control(PID *Gray, float now_Angle, float goal_Angle)
{
    PID_Calc(Gray, goal_Angle, now_Angle);
    return Gray->output;
}

//----------------------斜线角度环  BEGIN-------------------------------//
PID Xiexian_Angle;
void PID_Calc_Xiexian_Angle(PID *pid, float reference, float feedback)
{
    pid->lastError = pid->error;
    // 角度误差计算，确保在 [-180, 180] 范围内
    float error = reference - feedback;
    if (error > 180.0f)
        error -= 360.0f;
    else if (error < -180.0f)
        error += 360.0f;
    pid->error = error;
    // 比例项
    float pout = pid->kp * pid->error;
    // 微分项
    float dout = pid->kd * (pid->error - pid->lastError);
    // 积分项，限制积分防止饱和
    pid->integral += pid->ki * pid->error;
    if (pid->integral > pid->maxIntegral)
        pid->integral = pid->maxIntegral;
    else if (pid->integral < -pid->maxIntegral)
        pid->integral = -pid->maxIntegral;
    // 输出 = 比例 + 微分 + 积分
    pid->output = pout + dout + pid->integral;
    // 输出限幅
    if (pid->output > pid->maxOutput)
        pid->output = pid->maxOutput;
    else if (pid->output < -pid->maxOutput)
        pid->output = -pid->maxOutput;
}
float Xiexian_Angle_control(PID *Xiexian_Angle, float now_Angle, float goal_Angle)
{
    PID_Calc_Xiexian_Angle(Xiexian_Angle, goal_Angle, now_Angle);
    return Xiexian_Angle->output;
}